1. Field of the Invention
The present invention relates to an industrial robot, specifically to a technique of reducing shock when a robot collides with an obstacle and abnormal load is exerted on axes of the robot.
2. Description of the Related Art
FIG. 7 is a block diagram showing velocity loop control which is performed by servo controller for a servomotor for driving an axis of an industrial robot. A velocity deviation .epsilon. is obtained by subtracting a velocity feedback signal Vf sent from a velocity detector attached to a servomotor from a velocity command Vc outputted from a position loop control system or directly from a numerical controller. The value obtained by multiplying the integral of velocity deviation .epsilon. by an integral constant K1 (output of an integrator 100) and the value obtained by multiplying the velocity deviation .epsilon. by a proportional constant K2 (output of a proportional device 101) are added to obtain a torque command Tc. The servomotor is driven in accordance with the torque command Tc. Thus, the servomotor for driving a robot axis is generally drivingly controlled by the velocity loop control including proportional-plus-integral control.
Conventionally, in the control system as described above, when a collision of the robot with an obstacle is detected, each servomotor is drivingly controlled with a velocity command Vc turned to "0" so as to prevent damage such as breakage due to the collision.
In order to detect a collision, it can be adopted a method in which using a disturbance estimating observer 102 for estimating a disturbance torque Td based on a torque command Tc and a fed-back actual velocity Vf, it is determined that a collision has occurred when the estimated disturbance torque exceeds a predetermined value.
When a collision occurs and the velocity command Vc is turned to "0", a velocity feedback signal for the servomotor having a reversed sign is outputted to the velocity loop control system, and as a result, a torque command having a reversed sign, that is, a torque command which is to reverse the rotation of the servomotor is outputted to reduce shock due to the collision. Actually, the velocity loop system includes the integrator 100, and the influence of the integrator 100 needs to be taken into account. Here, in order to simplify the explanation, it is supposed that the influence of the integrator 100 is negligible.
FIGS. 6a to 6b are illustrations for explaining how a robot operates when a collision occurs and each servomotor is drivingly controlled with a velocity command Vc turned to "0". In FIGS. 6a to 6b, reference numeral 20 denotes an obstacle, 21 a hand attached to a wrist of the robot, and 22 an arm of the robot. Reference symbol Ma denotes a servomotor for driving the arm 22 (hereinafter referred to as "arm motor"), and reference symbol Mw denotes a servomotor for driving a wrist axis (hereinafter referred to as "wrist motor").
Suppose that the arm 22 is driven by the arm motor Ma in a direction indicated by arrow a in FIG. 6a. When an end of the hand 21 collides with the obstacle 20 as shown in FIG. 6b, the arm motor Ma continues producing torque and motor velocity having the same direction as before the collision (the counter-clockwise direction in FIG. 6b) and the hand 21 receives disturbance torque having the opposite direction (the clockwise direction in FIG. 6b) from the obstacle, as indicated in FIG. 6b.
When the collision is detected, the velocity command Vc for each motor is turned to "0". When the velocity command Vc is turned to "0", only the velocity feedback signal Vf is inputted to the velocity loop, and as a result, the torque command Tc for each motor has a direction opposite to that it had before, as described above. Thus, the torque having a direction opposite to that before, that is, the clockwise direction is produced by the arm motor as indicated in FIG. 6c, and as a result, the arm 22 and hand 21 recede from the obstacle 20 and the robot stops. However, since the hand 21 and arm 22 are pushed by repulsive force due to deflection of the obstacle caused by the collision, the arm motor has also a velocity having a direction such that the arm 22 recedes from the obstacle 20, that is, the clockwise direction, as indicated in FIG. 6c. Since that velocity is fed back to the velocity loop, the sign of the torque command Tc is reversed and toque having a direction such that the arm collides with the obstacle 20, that is, the counter-clockwise direction is produced by the arm motor Ma as indicated in FIG. 6d. Thus, a collision may occur again.